/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <map>
#include <string>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "slam/io/proto_stream.h"
#include "slam/io/proto_stream_deserializer.h"
#include "slam/io/submap_painter.h"
#include "slam/mapping/2d/probability_grid.h"
#include "slam/mapping/2d/submap_2d.h"
#include "slam/mapping/3d/submap_3d.h"
#include "slam_ros/ros_map.h"

namespace slam_ros {

class MapGenerator : public rclcpp::Node {
 public:
  explicit MapGenerator(const rclcpp::NodeOptions& options)
      : Node("map_generator", options) {
        
    this->declare_parameter<std::string>("pbstream_filename", "");
    this->declare_parameter<std::string>("map_filestem", "map");
    this->declare_parameter<double>("resolution", 0.05);

    pbstream_filename_ = this->get_parameter("pbstream_filename").as_string();
    map_filestem_ = this->get_parameter("map_filestem").as_string();
    resolution_ = this->get_parameter("resolution").as_double();


    if (pbstream_filename_.empty()) {
      RCLCPP_ERROR(this->get_logger(), "-pbstream_filename is missing.");
      rclcpp::shutdown();
      return;
    }

    if (map_filestem_.empty()) {
      RCLCPP_ERROR(this->get_logger(), "-map_filestem is missing.");
      rclcpp::shutdown();
      return;
    }

    Run();
  }

 private:
  void Run() {
    ::slam::io::ProtoStreamReader reader(pbstream_filename_);
    ::slam::io::ProtoStreamDeserializer deserializer(&reader);

    RCLCPP_INFO(this->get_logger(), "Loading submap slices from serialized data.");
    std::map<::slam::mapping::SubmapId, ::slam::io::SubmapSlice> submap_slices;
    ::slam::mapping::ValueConversionTables conversion_tables;
    ::slam::io::DeserializeAndFillSubmapSlices(
        &deserializer, &submap_slices, &conversion_tables);
    
    if (!reader.eof()) {
      RCLCPP_ERROR(this->get_logger(), "Failed to read pbstream file completely.");
      return;
    }

    RCLCPP_INFO(this->get_logger(), "Generating combined map image from submap slices.");
    auto result = ::slam::io::PaintSubmapSlices(submap_slices, resolution_);

    ::slam::io::StreamFileWriter pgm_writer(map_filestem_ + ".pgm");
    ::slam::io::Image image(std::move(result.surface));
    WritePgm(image, resolution_, &pgm_writer);

    const Eigen::Vector2d origin(
        -result.origin.x() * resolution_,
        (result.origin.y() - image.height()) * resolution_);

    ::slam::io::StreamFileWriter yaml_writer(map_filestem_ + ".yaml");
    WriteYaml(resolution_, origin, pgm_writer.GetFilename(), &yaml_writer);
  }

  std::string pbstream_filename_;
  std::string map_filestem_;
  double resolution_;
};

}  // namespace slam_ros

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions options;
  auto node = std::make_shared<slam_ros::MapGenerator>(options);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}